import sys
import time
sys.path.append("../")
from drivers.driver_serial import *

class DataReceiver:
    """传感器数据接收器，支持单次读取数据帧"""
    def __init__(self, device_name, band, timeout=0.5):
        """初始化接收器"""
        self.device_name = device_name
        self.band = band
        self.serial_device = None
        self.init_sensor()
    
    def parse_magnetic_data(self, frame):
        """解析16字节数据帧"""
        if len(frame) != 16:
            return None, "Invalid frame length"
        
        # 校验帧头
        if frame[0] != 0xAA or frame[1] != 0x55:
            return None, "Header mismatch"
        
        # 校验帧尾
        if frame[15] != 0x0D:
            return None, "Tail mismatch"
        
        # 计算校验和
        checksum = 0
        for i in range(2, 14):
            checksum ^= frame[i]
        
        if checksum != frame[14]:
            return None, f"Checksum error (calculated: {hex(checksum)}, received: {hex(frame[14])})"
        
        # 符号扩展函数
        def signed16(value):
            return value if value < 0x8000 else value - 0x10000
        
        # 解析传感器数据
        s1x_raw = (frame[2] << 8) | frame[3]
        s1y_raw = (frame[4] << 8) | frame[5]
        s1z_raw = (frame[6] << 8) | frame[7]
        s2x_raw = (frame[8] << 8) | frame[9]
        s2y_raw = (frame[10] << 8) | frame[11]
        s2z_raw = (frame[12] << 8) | frame[13]
        
        return {
            "s1x": signed16(s1x_raw), "s1y": signed16(s1y_raw), "s1z": signed16(s1z_raw),
            "s2x": signed16(s2x_raw), "s2y": signed16(s2y_raw), "s2z": signed16(s2z_raw)
        }, "OK"
    
    def read_single_frame(self, timeout=1.0, max_wait=5.0):
        """
        读取单个有效数据帧
        
        参数:
            timeout: 每次读取的超时时间(秒)
            max_wait: 最大等待时间(秒)
        
        返回:
            (s1x, s1y, s1z, s2x, s2y, s2z) 或 None
        """
        if not self.serial_device or not self.serial_device.is_open():
            print("错误：串口未打开")
            return None
        
        buffer = b""
        expected_frame_len = 16
        start_time = time.time()
        
        try:
            while time.time() - start_time < max_wait:
                # 读取串口数据
                data = self.serial_device.read_bytes()
                if data:
                    buffer += data
                    print(f"接收原始数据: {[hex(b) for b in buffer]}")
                    
                    # 帧同步处理
                    while True:
                        header_pos = buffer.find(b'\xAA\x55')
                        if header_pos == -1:
                            break
                        
                        if len(buffer) >= header_pos + expected_frame_len:
                            frame = buffer[header_pos:header_pos+expected_frame_len]
                            buffer = buffer[header_pos+expected_frame_len:]
                            
                            # 解析数据
                            result, status = self.parse_magnetic_data(frame)
                            if result:
                                print(f"[有效] S1: X={result['s1x']}, Y={result['s1y']}, Z={result['s1z']}")
                                print(f"[有效] S2: X={result['s2x']}, Y={result['s2y']}, Z={result['s2z']}")
                                return result['s1x'], result['s1y'], result['s1z'],result['s2x'], result['s2y'], result['s2z']
                            else:
                                print(f"[错误] {status}")
                        else:
                            break
                
                # 无数据时休眠，避免CPU占用过高
                time.sleep(timeout)
            
            print(f"超时未收到有效数据帧 (等待时间: {max_wait}秒)")
            return None
            
        except Exception as e:
            print(f"读取数据错误: {str(e)}")
            return None
    
    def init_sensor(self):
        """初始化串口设备"""
        self.serial_device = SerialDevice(self.device_name, self.band, timeout=0.5)
        success, msg = self.serial_device.open()
        if not success:
            print(f"打开端口失败: {msg}")
            sys.exit(1)
        print(f"串口 {self.device_name} 已打开，等待数据...")
    
    def close(self):
        """关闭串口"""
        if self.serial_device and self.serial_device.is_open():
            self.serial_device.close()
            print(f"串口 {self.device_name} 已关闭")